#include <config.h>
#include <mm/mem.h>
#include <types.h>
#include <arm32.h>
#include <suniv-clock.h>


#define DDRAM_START_ADDR  (0x80000000)
#define DDRAM_SIZE        CONFIG_DDRAM_SIZE


#define STACK_UND_SIZE     0x10000
#define STACK_ABT_SIZE     0x10000
#define STACK_IRQ_SIZE     0x10000
#define STACK_FIQ_SIZE     0x10000
#define STACK_SRV_SIZE     CONFIG_STACK_SIZE


extern size_t eb_system_heap;


static void clock_init(void)
{
    clock_cpu_config(CLK_CPU_SRC_OSC24M);
    sdelay(20); // wait for clock to stabilize

    /* set PLL_PERIPH_CLOCK = 24MHz * 25 / 1 = 600MHz */
    pll_periph_init(25, 1);
    /* enable PLL_PERIPH */
    clock_pll_enable(PLL_PERIPH);

    while(!clock_pll_is_ready(PLL_PERIPH)) {
        sdelay(1); // wait for PLL_PERIPH to be ready
    }

    /* set HCLK to 600MHz */
    clock_hclk_config(1);

    /* set AHB clock to PLL_PERIPH/3/1 = 200MHz */
    clock_ahb_config(3, 3, 1);

    /* set APB clock to AHB/2 = 100MHz */
    clock_apb_config(1);

    /* set CPU clock to 240MHz * 30 / 1 = 720MHz */
    pll_cpu_init(30, 1);
    /* enable PLL_CPU */
    clock_pll_enable(PLL_CPU);
    while(!clock_pll_is_ready(PLL_CPU)) {
        sdelay(1); // wait for PLL_CPU to be ready
    }

    /* set CPU clock source to PLL_CPU */
    clock_cpu_config(CLK_CPU_SRC_PLL_CPU);
    sdelay(20); // wait for CPU clock to stabilize
}


void eb_machine_init(void)
{
    void* start = &eb_system_heap;
	kmem_init(start, DDRAM_SIZE - STACK_SRV_SIZE - STACK_FIQ_SIZE \
                     - STACK_IRQ_SIZE - STACK_ABT_SIZE - STACK_UND_SIZE  \
                     - ((uint32_t)start - DDRAM_START_ADDR));

    /*  initialize the clock  */
    clock_init();

    /* enable all interrupts */
    arm32_interrupt_enable();
}
